package calculate;

import Jama.Matrix;


public class Triangulation {
		
	private BeaconData [] beacon;
	private Matrix matPosition = new Matrix(2, 2);

	public Triangulation(BeaconData[] beacon) {
		super();
		this.beacon = beacon;
		matPosition = calculatePosition();
		
	}
	
	public void setBeacon(BeaconData[] beacon){
		this.beacon = beacon;
	}
	
	public BeaconData getPosition(){
		BeaconData position = new BeaconData(matPosition.get(0, 0), matPosition.get(1,0));
		return position;
	}

	private Matrix calculatePosition(){
		double [][] a = new double [2][2];
		double [][] b = new double [2][1];
		
		a[0][0] = 2*(beacon[0].getX()-beacon[1].getX());
		a[0][1] = 2*(beacon[0].getY()-beacon[1].getY());
		a[1][0] = 2*(beacon[2].getX()-beacon[0].getX());
		a[1][1] = 2*(beacon[2].getY()-beacon[0].getY());

		
		b[0][0] = (beacon[1].getDistance() * beacon[1].getDistance()) - (beacon[0].getDistance() * beacon[0].getDistance())
					- (beacon[1].getX() * beacon[1].getX()) + (beacon[0].getX() * beacon[0].getX())
					- (beacon[1].getY() * beacon[1].getY()) + (beacon[0].getY() * beacon[0].getY());
		b[1][0] = (beacon[0].getDistance() * beacon[0].getDistance()) - (beacon[2].getDistance() * beacon[2].getDistance())
					- (beacon[0].getX() * beacon[0].getX()) + (beacon[2].getX() * beacon[2].getX())
					- (beacon[0].getY() * beacon[0].getY()) + (beacon[2].getY() * beacon[2].getY());
		
		Matrix matA = new Matrix(a);
		Matrix matB = new Matrix(b);
		
		Matrix matAInverse = matA.inverse();
		
		return matAInverse.times(matB);
	}
	
	public void showMatrix(Matrix mat){
		System.out.println();
		for (int i = 0; i < mat.getRowDimension(); i++) {
			for (int j = 0; j < mat.getColumnDimension(); j++) {
				System.out.print(mat.get(i, j)+"  ");
				
			}
			System.out.println();
		}
	}
}
